/*
ls_speed_mode.c
offer leisai motor driver for speed mode control
designed by lunanting

recoder
-------------------------
2020-11-25 15:22:53
create file
------------------------

*/
#include "ls_speed_mode.h"
#include "../dm556_driver.h"



/*
init a speed mode leisai motor object
param:
    id:node id
    motor_speed:motor speed, r/min
*/
int xp_LSspeed_init(int id,int motor_speed,LS_SPEED_class *motor){
    can_node_id_t can;
    int sta=0;
    s_profile_position_config config;

    memset(motor,0,sizeof(LS_SPEED_class));
    motor->can.port=1;
    motor->can.node=id;
    can.node=id;
    can.port=1;
    motor->motor_speed=motor_speed;

    if(xp_canopen_dev_manager_init()==-1){
        perr("can open init fail~");
        return -1;
    }
    aos_msleep(2000);
    sta=enable_dm556(&can);
    if(sta!=0){
        perr("dm556 enable fail~");
        return -2;
    }
    sta=profile_velocity_config(&can,2000,20000,20000);
    if(sta!=0){
        perr("config speed mode fail~");
        return -3;
    }

    motor->init=1;
    println("leisai motor id = %d speed mode init success~");
    return 0;
}



/*
speed mode run set
param:
    obj: motor obj
    sta: -1=reverse,0=stop,1=forward
return:
    0=success
*/
int xp_LSspeed_run_set(LS_SPEED_class *motor,int set){
    int sta=0,dir=-1;
    can_node_id_t can;

    if(motor==NULL){
        perr("motor obj == NULL");
        return -1;
    }
    if(motor->init==0){
        perr("motor not init~ id=%d",motor->can.node);
        return -2;
    }
    can.port=motor->can.port;
    can.node=motor->can.node;
    if(set==-1){
        dir=1;
        println("%d step motor reverse run,speed=%d",can.node,motor->motor_speed);
    }
    else if(set==1){
        dir=0;
        println("%d step motor forward run,speed=%d",can.node,motor->motor_speed);
    }
    else if(set==0){
        println("%d step motor stop",can.node);
        sta=slow_down(&can,50000);
        if(sta!=0){
            perr("slow down stop fail~ id=%d",can.node);
            return -5;
        }
    }
    if(dir!=-1){
        if(motor->dir!=0&&motor->dir!=set){
            sta=slow_down(&can,50000);
            if(sta!=0){
                perr("before change dir ,stop motor %d fail~",motor->can.node);
                return -7;
            }
            aos_msleep(1000);
        }
        sta=set_spin_direction(&can,dir);
        if(sta!=0){
            perr("set motor dir fail~ id=%d",can.node);
            return -3;
        }
        sta=set_target_velocity(&can,motor->motor_speed/60*2000);
        if(sta!=0){
            perr("set motor speed fail~ id=%d",can.node);
            return -4;
        }
    }
    if(set!=0){
        sta=clear_slow_down(&can);
        if(sta!=0){
            perr("clear motor slow down fail~ id=%d",can.node);
            return -5;
        }
    }
    motor->dir=set;
    return 0;
}

/*
update motor speed
param:
    ojb:motor obj
    speed: new speed
return:
    0=success
*/
int xp_LSspeed_update_speed(LS_SPEED_class *obj,int speed){
    can_node_id_t can;
    int sta=0;

    if(speed<=0||speed>1000){
        perr("update speed fail~ speed=%d",speed);
        return -1;
    }
    can.port=obj->can.port;
    can.node=obj->can.node;
    obj->motor_speed=speed;
    sta=set_target_velocity(&can,speed/60*2000);
    if(sta!=0){
        perr("set speed fail~");
        return -2;
    }
    return 0;

}



/***************************debug*************************/
static LS_SPEED_class step_motor={0};
//test init
int xp_LSspeed_test_int(void){
    xp_LSspeed_init(4,300,&step_motor);
}

//leisai speed motor debug
int xp_LSspeed_debug(char *type,char *fun,char *param){
    int i=0;
    static flag=0;

    if(flag==0){
        xp_LSspeed_test_int();
    }
    flag=1;
    if(strcmp(type,"LSspeed")!=0)return -1;
    else if(strcmp(fun,"test_speed")==0){
        i=atoi(param);
        if(i>0&&i<1000){
            println("LSspeed motor speed set as: %d",i);
            step_motor.motor_speed=i;
            xp_LSspeed_update_speed(&step_motor,i);
        }
        else{
            println("LSspeed motor speed set fail,%d ! in 0~1000",i);
        }
    }
    else if(strcmp(fun,"fw")==0){
        xp_LSspeed_run_set(&step_motor,1);
    }
    else if(strcmp(fun,"bk")==0){
        xp_LSspeed_run_set(&step_motor,-1);
    }
    else if(strcmp(fun,"stop")==0){
        xp_LSspeed_run_set(&step_motor,0);
    }
    else{
        return 0;
    }
    return 1;
}

//end of the file
